/****************************************************************************
 *  Copyright (C) 2018 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
/** @file chassis_task.c
 *  @version 1.1
 *  @date June 2017
 *
 *  @brief chassis control task
 *
 *  @copyright 2017 DJI RoboMaster. All rights reserved.
 *
 */

#include "chassis_task.h"
#include "gimbal_task.h"
#include "detect_task.h"
#include "comm_task.h"
#include "modeswitch_task.h"
#include "info_get_task.h"
#include "info_interactive.h"
#include "infantry_info.h"
#include "judgement_info.h"
#include "remote_ctrl.h"
#include "keyboard.h"
#include "pid.h"
#include "sys_config.h"
#include "stdlib.h"
#include "math.h"
#include "string.h"
#include "cmsis_os.h"
#include "function_mode_task.h"
#include "bsp_can.h"

/* chassis twist angle (degree)*/
#define TWIST_ANGLE    45
/* twist period time (ms) */
#define TWIST_PERIOD   1300
/* warning surplus energy */
#define WARNING_ENERGY 40.0f

UBaseType_t chassis_stack_surplus;

/* chassis task global parameter */
chassis_t chassis;

uint32_t chassis_time_last;
int chassis_time_ms;
extern TaskHandle_t can_msg_send_task_t;
uint32_t twist_count;

extern updown_island_mode_t updown_island_mode;

extern uint8_t rescue_camera_state;

//for j-scope debug
int chassis_vw_js = 0;
int chassis_vw_ref_js = 0;

uint8_t turn_back_flag;
float record_cur_gyro_angle;
void chassis_task(void const *argu)
{
  chassis_time_ms = HAL_GetTick() - chassis_time_last;
  chassis_time_last = HAL_GetTick();
  
//    get_chassis_info();
//    get_chassis_mode();

  switch (chassis.ctrl_mode)
  {
    case CHASSIS_STOP:
    {
      chassis_stop_handler();
    }break;

		case AUTO_UPDOWN_ISLAND_CHASSIS_ROTATE:
		{
			auto_move_handler();
		}break;
		
		case PICK_BOX_CHASSIS_MOVE:
		{
			auto_move_handler();
		}break;
		
		case SUPPLY_BALL_CHASSIS_MOVE:
		{
			auto_move_handler();
		}break;
		
		case RESCUE_CHASSIS_MOVE:
		{
			auto_move_handler();
		}break;
		
    case MANUAL_SEPARATE_GIMBAL:
    {
      separate_gimbal_handler();
    }break;

    default:
    {
      chassis_stop_handler();
    }break;
  }
	
	/************一键转身****************/
	if(turn_back_flag == 1)
	{
		if(rc.kb.bit.SHIFT)
		{
			record_cur_gyro_angle = gim.sensor.yaw_gyro_angle;
			turn_back_flag = 2;
		}
	}
	else if(turn_back_flag == 2)
	{
		if(abs(gim.sensor.yaw_gyro_angle - record_cur_gyro_angle) < 145	)
		{
			chassis.vw = 400;
		}
		else
		{
			chassis.vw = 0;
			turn_back_flag = 0;
		}
	}
	else if(turn_back_flag == 0)
	{
		if(!rc.kb.bit.SHIFT)
		{
			turn_back_flag = 1;
		}
	}
/***********************************/
	//for j-scope debug
  chassis_vw_ref_js = chassis.position_ref;
	chassis_vw_js = chassis.vw;
	
	//textend
  mecanum_calc(chassis.vx, chassis.vy, chassis.vw, chassis.wheel_spd_ref);

  for (int i = 0; i < 4; i++)
  {
    chassis.current[i] = pid_calc(&pid_spd[i], chassis.wheel_spd_fdb[i], chassis.wheel_spd_ref[i]);
  }
  
//  if (!chassis_is_controllable())
//  {
//    memset(chassis.current, 0, sizeof(chassis.current));
//  }
  
  memcpy(glb_cur.chassis_cur, chassis.current, sizeof(chassis.current));
	
  osSignalSet(can_msg_send_task_t, CHASSIS_MOTOR_MSG_SEND);
  osSignalSet(can_msg_send_task_t, MCU_COMMUNICATE_SIGNAL);
	
  chassis_stack_surplus = uxTaskGetStackHighWaterMark(NULL);
}


void chassis_stop_handler(void)
{
  chassis.vy = 0;
  chassis.vx = 0;
  chassis.vw = 0;
}


uint8_t chassis_move_protect_flag;
uint32_t chassis_move_protect_count;
void auto_move_handler(void)
{
	switch (function_mode.ctrl_mode)
	{
		case AUTO_UP_ISLAND:
		{
			if(updown_island_mode == begin_rotate)
			{
				chassis.vx = 0;
				chassis.vw = -400;
				chassis.vy = 0;
			}
			else 
			{
				chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
				chassis.vx = rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
				chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R ;
			}
		}break;
		
		case MANUAL_UP_DOWN_ISLAND:
		{
			chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
			chassis.vx = rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
			chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R ;
		}break;
		
		case AUTO_DOWN_ISLAND:
		{
			if(updown_island_mode == begin_rotate)
			{
				chassis.vx = 0;
				chassis.vw = 300;
				chassis.vy = 0;
			}
			else 
			{
				chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
				chassis.vx = rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
				chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R ;
			}
		}break;
		
		case PICK_UP_BOX:
		case PICK_DOWN_BOX:
		{
			if(mcu_rx_data.pick_box_flag == 1)    //夹取时底盘拨杆反弹保护
			{
//				if(chassis_move_protect_flag == 0)
//				{
					chassis.vx = -rm.vy * CHASSIS_RC_MOVE_RATIO_Y*0.2 + km.vy * CHASSIS_KB_MOVE_RATIO_Y*0.2;
					chassis.vy = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
					chassis.vw = 0;
//					chassis_move_protect_flag = 1;
//				}
//				if(chassis_move_protect_flag == 1)
//				{
//					chassis.vy = -rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
//					chassis.vx = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
//					chassis.vw = 0;
//					chassis_move_protect_count = HAL_GetTick();
//					chassis_move_protect_flag = 2;
//				}
//				else if(chassis_move_protect_flag == 2)
//				{
//					chassis.vy = -rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
//					chassis.vx = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
//					chassis.vw = 0;
//					if(HAL_GetTick() - chassis_move_protect_count > 1500)
//					{
//						chassis_move_protect_flag = 0;
//					}
//				}
//				else if(chassis_move_protect_flag == 0)
//				{
//					chassis_move_protect_flag = 0;
//					chassis.vy = -rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
//					chassis.vx = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
//					chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R;
//				}
			}
			else 
			{
//				chassis_move_protect_flag = 0;
				chassis.vx = -rm.vy * CHASSIS_RC_MOVE_RATIO_Y*0.2 + km.vy * CHASSIS_KB_MOVE_RATIO_Y*0.2;
				chassis.vy = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
				chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R;
			}
		}break;
		case SUPPLY_BALL:
		{
			chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y - km.vy * CHASSIS_KB_MOVE_RATIO_Y;
			chassis.vx = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
			chassis.vw = -rm.vw * CHASSIS_RC_MOVE_RATIO_R;
		}break;
		case RESCUE_MODE:
		{
			if(rescue_camera_state == 1)
			{
				chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y - km.vy * CHASSIS_KB_MOVE_RATIO_Y;
				chassis.vx = -rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
				chassis.vw = -rm.vw * CHASSIS_RC_MOVE_RATIO_R;
			}
			else if(rescue_camera_state == 0)
			{
				chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
				chassis.vx = rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;
				chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R;
			}
		}break;
	}
}

/*底盘直线行驶漂移角度记录*/
float record_angle;
float drift_angle;
float drift_repay;
float drift_pid[3] = {6, 0.2, 7};
float drift_rate_param =0;
uint8_t out_bunker_state;
float out_bunker_pit_angle;
float out_bunker_yaw_angle;
uint32_t out_bunker_sign_moment;

float out_bunker_vx;
float out_bunker_vy;
float out_bunker_vw;
float x_out_bunker_spd = 1100;
float w_out_bunker_spd = 300;
void separate_gimbal_handler(void)
{
	if(chassis.last_ctrl_mode != chassis.ctrl_mode)
	{
		out_bunker_vx = 0;
		out_bunker_vy = 0;
		out_bunker_vw = 0;
		out_bunker_state = 0;
	}
	chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y + out_bunker_vy;
	chassis.vx = rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X + out_bunker_vx;
	chassis.vw = rm.vw * CHASSIS_RC_MOVE_RATIO_R - drift_rate_param * drift_repay + out_bunker_vw;
//	if(out_bunker_state == 0)
//	{
//		PID_struct_init(&pid_drift, POSITION_PID, 1000, 0, drift_pid[0], drift_pid[1], drift_pid[2]); 
//		if((rm.vx == 0 && rm.vy == 0) || rm.vw != 0)
//		{
//			record_angle = gim.sensor.yaw_gyro_angle;
//		}
//		
//		if(rm.vw == 0)
//		{
//			//drift_angle = gim.sensor.yaw_gyro_angle - record_angle
//			drift_repay = pid_calc(&pid_drift, record_angle, gim.sensor.yaw_gyro_angle);
//		}
//		
//	
//	}
/*一键出碉堡*/
	if(rc.iw>0x1800)
	{
		out_bunker_state = 1;
	}
	else if(rc.iw == 0x0694)
	{
		out_bunker_vx = 0;
		out_bunker_vy = 0;
		out_bunker_vw = 0;
		out_bunker_state = 0;
	}
	
	if(out_bunker_state == 1)
	{	
		out_bunker_vx = x_out_bunker_spd;
		out_bunker_vy = 0;
		out_bunker_vw = 0;
		out_bunker_pit_angle = gim.sensor.pit_gyro_angle;
		out_bunker_state = 2;
	}
	if(out_bunker_state == 2)
	{
		if(abs(gim.sensor.pit_gyro_angle - out_bunker_pit_angle) > 12)
		{
			out_bunker_state = 3;
		}
	}
	if(out_bunker_state == 3)
	{
		out_bunker_pit_angle = gim.sensor.pit_gyro_angle;
		if(abs(out_bunker_pit_angle) < 8)
		{
			out_bunker_sign_moment = HAL_GetTick();
			out_bunker_state = 4;
		}
	}
	if(out_bunker_state == 4)
	{
		if(HAL_GetTick() - out_bunker_sign_moment > 100)
		{
			out_bunker_vx = x_out_bunker_spd;
			out_bunker_vy = 0;
			out_bunker_vw = w_out_bunker_spd;
			out_bunker_yaw_angle = gim.sensor.yaw_gyro_angle;
			out_bunker_state = 5;
		}
	}
	if(out_bunker_state == 5)
	{
		if(abs(gim.sensor.yaw_gyro_angle - out_bunker_yaw_angle) > 65)
		{
			out_bunker_vx = x_out_bunker_spd;
			out_bunker_vy = 0;
			out_bunker_vw = 0;
			out_bunker_state = 6;
		}
	}
	if(out_bunker_state == 6)
	{	
		out_bunker_vx = x_out_bunker_spd;
		out_bunker_vy = 0;
		out_bunker_vw = 0;
		out_bunker_pit_angle = gim.sensor.pit_gyro_angle;
		out_bunker_state = 7;
	}
	if(out_bunker_state == 7)
	{
		if(abs(gim.sensor.pit_gyro_angle - out_bunker_pit_angle) > 12)
		{
			out_bunker_state = 8;
		}
	}
	if(out_bunker_state == 8)
	{
		out_bunker_pit_angle = gim.sensor.pit_gyro_angle;
		if(abs(out_bunker_pit_angle) < 8)
		{
			out_bunker_sign_moment = HAL_GetTick();
			out_bunker_state = 9;
		}
	}
	if(out_bunker_state == 9)
	{
		if(HAL_GetTick() - out_bunker_sign_moment > 5)
		{
			out_bunker_vx = 0;
			out_bunker_vy = 0;
			out_bunker_vw = -w_out_bunker_spd;
			out_bunker_yaw_angle = gim.sensor.yaw_gyro_angle;
			out_bunker_state = 10;
		}
	}
	if(out_bunker_state == 10)
	{
		if(abs(gim.sensor.yaw_gyro_angle - out_bunker_yaw_angle) > 93)
		{
			out_bunker_vx = x_out_bunker_spd;
			out_bunker_vy = 0;
			out_bunker_vw = 0;
			out_bunker_state = 11;
			out_bunker_sign_moment = HAL_GetTick();
		}
	}
	if(out_bunker_state == 11)
	{
		if(HAL_GetTick() - out_bunker_sign_moment > 2000)
		{
			out_bunker_vx = 0;
			out_bunker_vy = 0;
			out_bunker_vw = 0;
			out_bunker_state = 0;
		}
	}
}

//void follow_gimbal_handler(void)
//{
//	twist_count = 0;
//	
//  chassis.position_ref = 0;
//		
//  chassis.vy = rm.vy * CHASSIS_RC_MOVE_RATIO_Y + km.vy * CHASSIS_KB_MOVE_RATIO_Y;
//  chassis.vx = rm.vx * CHASSIS_RC_MOVE_RATIO_X + km.vx * CHASSIS_KB_MOVE_RATIO_X;

//  if (chassis.follow_gimbal)
//    chassis.vw = -pid_calc(&pid_chassis_angle, gim.sensor.yaw_relative_angle, chassis.position_ref);
//  else
//    chassis.vw = 0;
//}

/**
  * @brief mecanum chassis velocity decomposition
  * @param input : ↑=+vx(mm/s)  ←=+vy(mm/s)  ccw=+vw(deg/s)
  *        output: every wheel speed(rpm)
	* @trans 输入：		前后左右的量
	*				 输出：		每个轮子对应的速度
  * @note  1=FR 2=FL 3=BL 4=BR
	* @work	 分析演算公式计算的效率
  */
int rotation_center_gimbal = 0;
void mecanum_calc(float vx, float vy, float vw, int16_t speed[])
{
  static float rotate_ratio_fr;
  static float rotate_ratio_fl;
  static float rotate_ratio_bl;
  static float rotate_ratio_br;
  static float wheel_rpm_ratio;
  
  taskENTER_CRITICAL();
  if(chassis.ctrl_mode == DODGE_MODE)
  {
    chassis.rotate_x_offset = GIMBAL_X_OFFSET;
    chassis.rotate_y_offset = 0;
  }
  else
  {
    if (rotation_center_gimbal)
    {
      chassis.rotate_x_offset = glb_struct.gimbal_x_offset;
      chassis.rotate_y_offset = glb_struct.gimbal_y_offset;
    }
    else
    {
      chassis.rotate_x_offset = 0;
      chassis.rotate_y_offset = 0;
    }
  }
  //@work
  rotate_ratio_fr = ((glb_struct.wheel_base+glb_struct.wheel_track)/2.0f \
                      - chassis.rotate_x_offset + chassis.rotate_y_offset)/RADIAN_COEF;
  rotate_ratio_fl = ((glb_struct.wheel_base+glb_struct.wheel_track)/2.0f \
                      - chassis.rotate_x_offset - chassis.rotate_y_offset)/RADIAN_COEF;
  rotate_ratio_bl = ((glb_struct.wheel_base+glb_struct.wheel_track)/2.0f \
                      + chassis.rotate_x_offset - chassis.rotate_y_offset)/RADIAN_COEF;
  rotate_ratio_br = ((glb_struct.wheel_base+glb_struct.wheel_track)/2.0f \
                      + chassis.rotate_x_offset + chassis.rotate_y_offset)/RADIAN_COEF;

  wheel_rpm_ratio = 60.0f/(glb_struct.wheel_perimeter*CHASSIS_DECELE_RATIO);
  taskEXIT_CRITICAL();
  
  
  VAL_LIMIT(vx, -MAX_CHASSIS_VX_SPEED, MAX_CHASSIS_VX_SPEED);  //mm/s
  VAL_LIMIT(vy, -MAX_CHASSIS_VY_SPEED, MAX_CHASSIS_VY_SPEED);  //mm/s
  VAL_LIMIT(vw, -MAX_CHASSIS_VR_SPEED, MAX_CHASSIS_VR_SPEED);  //deg/s
  
  int16_t wheel_rpm[4];
  float   max = 0;
  
  wheel_rpm[0] = (-vx - vy - vw * rotate_ratio_fr) * wheel_rpm_ratio;
  wheel_rpm[1] = ( vx - vy - vw * rotate_ratio_fl) * wheel_rpm_ratio;
  wheel_rpm[2] = ( vx + vy - vw * rotate_ratio_bl) * wheel_rpm_ratio;
  wheel_rpm[3] = (-vx + vy - vw * rotate_ratio_br) * wheel_rpm_ratio;

  //find max item
  for (uint8_t i = 0; i < 4; i++)
  {
    if (abs(wheel_rpm[i]) > max)
      max = abs(wheel_rpm[i]);
  }
  //equal proportion
  if (max > MAX_WHEEL_RPM)
  {
    float rate = MAX_WHEEL_RPM / max;
    for (uint8_t i = 0; i < 4; i++)
      wheel_rpm[i] *= rate;
  }
  memcpy(speed, wheel_rpm, 4*sizeof(int16_t));
}



/**
  * @brief  nitialize chassis motor pid parameter
  * @usage  before chassis loop use this function
  */
void chassis_param_init(void)
{
  memset(&chassis, 0, sizeof(chassis_t));
  
  chassis.ctrl_mode      = CHASSIS_STOP;
  chassis.last_ctrl_mode = CHASSIS_RELAX;
  
#ifdef CHASSIS_EC60
  for (int k = 0; k < 4; k++)
  {
    PID_struct_init(&pid_spd[k], POSITION_PID, 10000, 2500, 55, 1.0, 0);
  }
#else
  for (int k = 0; k < 4; k++)
  {
    PID_struct_init(&pid_spd[k], POSITION_PID, 10000, 500, 4.5f, 0.05, 0);
  }
#endif
  
  PID_struct_init(&pid_chassis_angle, POSITION_PID, MAX_CHASSIS_VR_SPEED, 50, 14.0f, 0.0f, 50.0f);
  
  glb_struct.chassis_config = NO_CONFIG;
  glb_struct.gimbal_config  = NO_CONFIG;
  
  memset(&pc_recv_mesg.structure_data, 0, sizeof(pc_recv_mesg.structure_data));
}

//#if 0
//int32_t total_cur_limit;
//int32_t total_cur;
//void power_limit_handler(void)
//{
//  if (g_err.list[JUDGE_SYS_OFFLINE].err_exist)
//  {
//    //judge system offline, mandatory limit current
//    total_cur_limit = 8000;
//  }
//  else
//  {
//    if (judge_recv_mesg.game_information.remain_power < WARNING_ENERGY)
//      total_cur_limit = ((judge_recv_mesg.game_information.remain_power * \
//                          judge_recv_mesg.game_information.remain_power)/ \
//                          (WARNING_ENERGY*WARNING_ENERGY)) * 40000;
//    else
//      total_cur_limit = 40000;
//  }
//  
//  total_cur = abs(chassis.current[0]) + abs(chassis.current[1]) + \
//              abs(chassis.current[2]) + abs(chassis.current[3]);
//  
//  if (total_cur > total_cur_limit)
//  {
//    chassis.current[0] = chassis.current[0] / total_cur * total_cur_limit;
//    chassis.current[1] = chassis.current[1] / total_cur * total_cur_limit;
//    chassis.current[2] = chassis.current[2] / total_cur * total_cur_limit;
//    chassis.current[3] = chassis.current[3] / total_cur * total_cur_limit;
//  }

//}
//#endif


